#!/usr/bin/python3

import roslib
import rospy
import math
import tf
import tf2_ros
from geometry_msgs.msg import Pose, Twist, TransformStamped, PointStamped, PoseStamped
import time
import numpy as np
import csv
from std_msgs.msg import Float64MultiArray
import copy
from tf2_geometry_msgs import tf2_geometry_msgs
from gazebo_msgs.srv import SpawnModel, SpawnModelRequest, SpawnModelResponse
from gazebo_msgs.srv import SetModelState, SetModelStateRequest, SetModelStateResponse
from p903_simulation.srv import TransformPart,TransformPartRequest,TransformPartResponse

class tail_targeting_control():
    def __init__(self,nodename):
        rospy.init_node(nodename)

        self.srv_modulating_part = rospy.Service("transform_part",TransformPart, self.modulate_part_pose)
        self.srv_model_state = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState)
        # self.srv_replace_product = rospy.Service("replace_product",ProductOnAGVControl, self.replace_product_cb)
        # self.srv_modulating_agv_product = rospy.Service("modulate_product_with_agv",ProductOnAGVControl, self.modulate_product_on_agv_cb)

        self.tfBuffer = tf2_ros.Buffer()
        self.tfListener = tf2_ros.TransformListener(self.tfBuffer) 

        self.tfBroadcaster=tf2_ros.TransformBroadcaster()

    def modulate_part_pose(self,req):
        # print(req)
        fsuccess = True
        parts = {}
        for reqi in req.targetTransform:
            ps = PoseStamped()
            ps.header.frame_id = reqi.header.frame_id
            ps.header.stamp = rospy.Time(0,0)
            ps.pose.position.x = reqi.transform.translation.x
            ps.pose.position.y = reqi.transform.translation.y
            ps.pose.position.z = reqi.transform.translation.z

            ps.pose.orientation.x = reqi.transform.rotation.x
            ps.pose.orientation.y = reqi.transform.rotation.y
            ps.pose.orientation.z = reqi.transform.rotation.z
            ps.pose.orientation.w = reqi.transform.rotation.w

            newPose = self.tfBuffer.transform(ps, 'world')
            parts[reqi.child_frame_id] = newPose

        print(parts)
        for name, newPose in parts.items():
            sent_req = SetModelStateRequest()
            sent_req.model_state.model_name = name
            sent_req.model_state.reference_frame = 'ground_plane'
            sent_req.model_state.pose = newPose.pose

            resp = self.srv_model_state.call(sent_req)
            fsuccess = fsuccess and resp.success

        return TransformPartResponse(success=fsuccess)

    # def replace_product_cb(self,req):
    #     if(not rospy.has_param("/all_products/all_product_description")):
    #         return ProductOnAGVControlResponse(success = False)
        
    #     desc = rospy.get_param("/all_products/all_product_description")
    #     all_names = [i['urdf_description']['prefix'] for i in desc]
    #     if(not req.productName in all_names):
    #         return ProductOnAGVControlResponse(success = False)

    #     srv_load_product = rospy.ServiceProxy("/all_products/replace_gazebo_model", SpawnModel)

    #     call_req = SpawnModelRequest()
    #     call_req.model_name = req.agvName + ":" + req.productName
    #     call_req.reference_frame = req.agvName
    #     # call_req.initial_pose = call_resp.pose.pose
    #     call_resp = srv_load_product.call(call_req)

    #     return ProductOnAGVControlResponse(success = True)

    # def load_product_on_agv_cb(self,req):

    #     if(not rospy.has_param("/all_products/all_product_description")):
    #         return ProductOnAGVControlResponse(success = False)
        
    #     desc = rospy.get_param("/all_products/all_product_description")
    #     all_names = [i['urdf_description']['prefix'] for i in desc]
    #     if(not req.productName in all_names):
    #         return ProductOnAGVControlResponse(success = False)

    #     srv_get_ball_position = rospy.ServiceProxy("/all_products/get_support_ball_positions", GetKeyFeatureMeasure)
    #     srv_get_support_pose = rospy.ServiceProxy("/"+ req.agvName + "/get_support_pose", GetPoseFromPoints)
    #     srv_load_product = rospy.ServiceProxy("/all_products/replace_gazebo_model", SpawnModel)

    #     call_req = GetKeyFeatureMeasureRequest()
    #     call_req.key_feature_name.data = "all_products/" + req.productName
    #     call_resp = srv_get_ball_position.call(call_req)

    #     call_req = GetPoseFromPointsRequest()
    #     call_req.points = call_resp.points
    #     call_resp = srv_get_support_pose.call(call_req)

    #     print(call_resp.pose)

    #     call_req = SpawnModelRequest()
    #     call_req.model_name = "all_products/" + req.productName + ":" + req.productName
    #     call_req.reference_frame = req.agvName
    #     call_req.initial_pose = call_resp.pose.pose
    #     call_resp = srv_load_product.call(call_req)

    #     return ProductOnAGVControlResponse(success = True)

    def run(self):
        rate = rospy.Rate(10)
        while not rospy.is_shutdown():
            rate.sleep()

if __name__ == '__main__':
    service = tail_targeting_control('tail_targeting_control')
    service.run()
